/*
 * pid_control.c
 *
 *  Created on: Sep 12, 2023
 *      Author: smile
 */
#include "pid_control.h"

PID_Def curret_Id_pid;
PID_Def curret_Iq_pid;
PID_Def speed_loop_pid;
PID_Def position_loop_pid;



PID_HANDLE id_pid;
PID_HANDLE iq_pid;

PID_HANDLE speed_pid;








float pidProcess(P_PID_HANDLE phdl, float input, float measure)
{
    float output = 0;
    float error = input - measure;         //计算误差

    phdl->sum_error += error;

    //误差积分限幅
    if(phdl->enable_lim_sum_error == 1 && fabs(phdl->sum_error) > phdl->lim_sum_error)
    {
        if(phdl->sum_error > 0)
            phdl->sum_error = phdl->lim_sum_error;
        else
            phdl->sum_error = -phdl->lim_sum_error;
    }

    output =        phdl->kp * error
                +   phdl->ki * phdl->sum_error
                +   phdl->kd * (error - phdl->last_error) ;
		phdl->kp_output = phdl->kp * error;
		phdl->ki_output = phdl->ki * phdl->sum_error;
		phdl->kd_output = phdl->kd * (error - phdl->last_error);

		phdl->error_dec = error - phdl->last_error;
		
    //输出限幅
    if(phdl->enable_lim_ouput == 1 && fabs(output) > phdl->lim_output)
    {
        if(output > 0)
            output = phdl->lim_output;
        else
            output = -phdl->lim_output;
    }

    //更新误差值
    phdl->last_error = error;

    //是否使能输出
    if(phdl->enable == 1)
	{
		phdl->pid_output = output;
		return output;
	}
    else
	{
		phdl->pid_output = 0;
		return 0;
	}
}
























void PID_init(void)
{
  
  id_pid.kp=2.4f;        //2.0             2.4
  id_pid.ki=0.2f;       //0.01    0.05     0.2
  id_pid.kd=0.0f;
  
  iq_pid.kp=2.40f;        //2.0
  iq_pid.ki=0.20f;        //0.01
  iq_pid.kd=0.0f;
  
  
  
  speed_pid.kp=0.003f;        //0.01
  speed_pid.ki=0.000f;
  speed_pid.kd=0.0f;
  
  
  id_pid.lim_output=20.0f;
  id_pid.lim_sum_error=15.0f;
  iq_pid.lim_output=20.0f;
  iq_pid.lim_sum_error=15.0f;
  
  speed_pid.lim_output=10.0f;
  speed_pid.lim_sum_error=8.5f;
  
  
  id_pid.enable=1;
  id_pid.enable_lim_ouput=1;
  id_pid.enable_lim_sum_error=1;
  
  iq_pid.enable=1;
  iq_pid.enable_lim_ouput=1;
  iq_pid.enable_lim_sum_error=1;
  
  speed_pid.enable=1;
  speed_pid.enable_lim_ouput=1;
  speed_pid.enable_lim_sum_error=1;
  
  
	curret_Id_pid.Kp = 0.9f;
	curret_Id_pid.Ki = 0.001f;
	curret_Id_pid.Kd = 0;
	curret_Id_pid.Integral_max = 6.93f;


	curret_Iq_pid.Kp = curret_Id_pid.Kp;
	curret_Iq_pid.Ki = curret_Id_pid.Ki;
	curret_Iq_pid.Kd = curret_Id_pid.Kd;
	curret_Iq_pid.Integral_max = curret_Id_pid.Integral_max;

	speed_loop_pid.Kp = 0.00255f;
	speed_loop_pid.Ki = 0.00015f;
	speed_loop_pid.Kd = 0;
	speed_loop_pid.Integral_max = 100.0f;

	position_loop_pid.Kp = 0.001f;
	position_loop_pid.Ki = 0.0003f;
	position_loop_pid.Kd = 0.0001;
	position_loop_pid.Integral_max = 100.0f;

}


float PID_control(PID_Def *PID,float set_Val,float Actual_Val)
{

	PID->SetVal=set_Val;
	PID->ActualVal=Actual_Val;
	PID->err=set_Val-Actual_Val;
	PID->Integral += PID->err;

	PID->Integral=LIMIT((PID->Integral),-PID->Integral_max,PID->Integral_max);

	PID->out=PID->Kp*PID->err + PID->Ki*(PID->Integral) + PID->Kd*(PID->err-PID->err_next);
	PID->err_next=PID->err;
    return PID->out;
}


//位置式PID
float position_PID_control(PID_Def *PID,float set_Val,float Actual_Val)
{

	float inter;
	PID->SetVal= set_Val;
	PID->ActualVal=Actual_Val;

	PID->err=set_Val-Actual_Val;
	if(fabs(PID->err)>=POSITION_ACCURACY) //位置精度
	{
        inter = PID->err;
        PID->Integral += inter;
        PID->Integral = LIMIT((PID->Integral),-100.0f,100.0f);
        PID->out = PID->Kp*PID->err + PID->Ki*(PID->Integral) + PID->Kd*(PID->err - PID->err_next);
        PID->err_next = PID->err;

	}
	else
	{
		PID->out = 0.0f;
	}
    return PID->out;
}

